﻿/* ***************************************************************************
 *  This file is part of the C# port of the Invivo AI challenge simulation environment.
 *  Copyright 2010 Colin Green
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program .  If not, see <http://www.gnu.org/licenses/>.
 */
namespace FunkAIChallenge
{
    /// <summary>
    /// Simple PlayerController example that maintains a straight player arm.
    /// Applies torques to direct the joints towards the required angles for a straight arm.
    /// </summary>
    public class StraightArmController : PlayerController
    {
        /// <summary>
        /// Construct with the provided player interface.
        /// </summary>
        public StraightArmController(PlayerInterface iface) 
            : base(iface) 
        {}

        /// <summary>
        /// Perform one controller step. Typically consisting of reading world and player arm state
        /// from the player interface and updating the torques applied at each player arm joint.
        /// </summary>
        public override void Step()
        {
            // Guide all joint angles to form a straight arm.
            float[] jointAngleArr = _iface.JointAngleArr;
            JointControl(jointAngleArr, 0, 0f);
            JointControl(jointAngleArr, 1, 0f);
            JointControl(jointAngleArr, 2, 0f);
        }

        private void JointControl(float[] jointAngleArr, int idx, float targetAngle)
        {
            float angle = jointAngleArr[idx];
            float error = targetAngle - angle;
            _iface.SetJointTorque(idx, error*10f);
        }
    }
}
